#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
class MyMath
{

	// Determine whether a ray intersect with a triangle
	// Parameters
	// orig: origin of the ray
	// dir: direction of the ray
	// v0, v1, v2: vertices of triangle
	// t(out): weight of the intersection for the ray
	// u(out), v(out): barycentric coordinate of intersection

public :
	bool IntersectTriangle(const glm::vec3& orig, const glm::vec3& dir, glm::vec3& v0, glm::vec3& v1, glm::vec3& v2, float* t, float* u, float* v)
	{
		// E1
		glm::vec3 E1 = v1 - v0;

		// E2
		glm::vec3 E2 = v2 - v0;

		// P
		glm::vec3 P = glm::cross(dir, E2);

		// determinant
		float det = glm::dot(E1, P);

		// keep det > 0, modify T accordingly
		glm::vec3 T;
		if (det > 0)
		{
			T = orig - v0;
		}
		else
		{
			T = v0 - orig;
			det = -det;
		}

		// If determinant is near zero, ray lies in plane of triangle
		if (det < 0.0001f)
			return false;

		// Calculate u and make sure u <= 1
		*u = glm::dot(T, P);
		if (*u < 0.0f || *u > det)
			return false;

		// Q
		glm::vec3 Q = glm::cross(T, E1);

		// Calculate v and make sure u + v <= 1
		*v = glm::dot(dir, Q);
		if (*v < 0.0f || *u + *v > det)
			return false;

		// Calculate t, scale parameters, ray intersects triangle
		*t = glm::dot(E2, Q);

		float fInvDet = 1.0f / det;
		*t *= fInvDet;
		*u *= fInvDet;
		*v *= fInvDet;

		return true;
	}

	// Determine whether point P in triangle ABC
	bool PointinTriangle(glm::vec3 A, glm::vec3 B, glm::vec3 C, glm::vec3 P)
	{
		glm::vec3 v0 = C - A;
		glm::vec3 v1 = B - A;
		glm::vec3 v2 = P - A;

		float dot00 = glm::dot(v0, v0);
		float dot01 = glm::dot(v0, v1);
		float dot02 = glm::dot(v0, v2);
		float dot11 = glm::dot(v1, v1);
		float dot12 = glm::dot(v1, v2);

		float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01);

		float u = (dot11 * dot02 - dot01 * dot12) * inverDeno;
		if (u < 0 || u > 1) // if u out of range, return directly
		{
			return false;
		}

		float v = (dot00 * dot12 - dot01 * dot02) * inverDeno;
		if (v < 0 || v > 1) // if v out of range, return directly
		{
			return false;
		}

		return u + v <= 1;
	}

	double DistanceOfPointToLine(glm::vec3 a, glm::vec3 b, glm::vec3 s)
	{
		double ab = glm::distance(a, b);
		double as = glm::distance(a, s);
		double bs = glm::distance(b, s);
		double cos_A = glm::dot(normalize(b - a), normalize(s - a));
		double sin_A = sqrt(1 - pow(cos_A, 2.0));
		return as * sin_A;
	}
};